
#pragma once

#include <cassert>
#include <deque>
#include <algorithm>

using namespace std;

template<class SupportType, class DataType, float (*InnerProduct)(const DataType&, const DataType&)>
class SplineData {
    deque<SupportType> mTs;     // knots
    deque<DataType> mPts;       // data points

    mutable vector<DataType> mBlendPts; // points for blending

    const int mDegree;

public:

    SplineData(SupportType zero, int degree)
        : mDegree(degree)
        , mTs(degree+1, zero)
    {
        mBlendPts.resize(degree+1);
    }

    SplineData& operator=(const SplineData& src)
    {
        const_cast<int>(mDegree)=src.mDegree;
        mTs=src.mTs;
        mPts=src.mPts;

        return this;
    }

    void AddData(SupportType t, const DataType& pt)
    {
        assert(mTs.back() <= t);

        if(mPts.size()>1) {
            deque<DataType>::iterator last=mPts.end()-1;
            deque<DataType>::const_iterator prevLast=last-1;
            DataType diamVec=pt-*prevLast;
            DataType center=(pt+*prevLast)*.5f;
            float radiusSq=InnerProduct(diamVec, diamVec)*.25f;
            DataType candDist=*last-center;
            float distSq=InnerProduct(candDist, candDist);

            if(radiusSq < distSq)
                *last=center+candDist*sqrtf(radiusSq/distSq);
        }

        mPts.push_back(pt);
        mTs.push_back(t);
    }

    DataType GetData(SupportType t) const
    {
        assert(mPts.size()>0);

        if(mPts.size()<mDegree+1)
            return mPts.front();

        deque<SupportType>::const_iterator kt=--upper_bound(mTs.begin()+mDegree+1, mTs.end()-mDegree, t);
        if(kt==mTs.end()-mDegree-1 || kt==mTs.begin()+mDegree)
            t=*kt;  // clamp to valid range
        int k=(int)(kt-mTs.begin());
        int s;
        for(s=0;*kt==t;--kt) {
            s++;    // count knot multiplicity
            if(kt==mTs.begin())
                break;
        }
        s=min(s,mDegree);   // clamp knot multiplicity

#if 0
        // probably don't need it
        if(k-s+1>mPts.size())
            return *(mPts.end()-3);
#endif
        copy(mPts.begin()+(k-mDegree), mPts.begin()+(k-s+1), mBlendPts.begin());

        for(int r=1;r<=mDegree-s;r++)
            for(int i=k-mDegree+r,j=1;i<=k-s;i++,j++) {
                float x=((float)(t-mTs[i]))/((float)(mTs[i+mDegree-r+1]-mTs[i]));
                mBlendPts[j-1]=mBlendPts[j-1]*(1.f-x) + mBlendPts[j]*x;
            }

        return mBlendPts.front();
    }

    void CleanupUntil(SupportType t)
    {
        deque<SupportType>::iterator it=upper_bound(mTs.begin(), mTs.end(), t);

        int pos=it-mTs.begin();
        if(pos>2) {
            mTs.erase(mTs.begin()+mDegree+1, it-3);
            pos-=3;
            mPts.erase(mPts.begin(), mPts.begin()+pos);
        }
    }
};
